/**
 * @file screen_driver.h
 * @author circleup (circleup@foxmial.com)
 * @brief rviz 显示
 * @version 0.1
 * @date 2020-07-14
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#ifndef _SCREEN_DRIVER_H_
#define _SCREEN_DRIVER_H_

#include <vector>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/Path.h>

#include "ros/ros.h"

class point
{
public:

  /**
   * @brief 线程 point_rviz_handle
   * 
   */
  ros::NodeHandle point_rviz_handle;

  /**
   * @brief 消息发布 point_rviz_pub
   * 
   */
  ros::Publisher point_rviz_pub;

  /**
   * @brief rviz 消息变量
   * 
   */
  visualization_msgs::Marker point_rviz;

  /**
   * @brief 临时点
   * 
   */
  geometry_msgs::Point this_points;

  /**
   * @brief Construct a new point object
   * 
   */
  point(void);

  /**
   * @brief Destroy the point object
   * 
   */
  ~point(void);

  /**
   * @brief 添加要显示的点
   * 
   */
  void AddPoint(void);

  /**
   * @brief 将点发布到 rviz 中
   * 
   */
  void publish_to_rviz(void);
};

class path
{
public:
  /**
   * @brief 线程 path_rviz_handle
   * 
   */
  ros::NodeHandle path_rviz_handle;

  /**
   * @brief 消息发布 path_rviz_pub
   * 
   */
  ros::Publisher path_rviz_pub;

  /**
   * @brief rviz 消息变量
   * 
   */
  nav_msgs::Path path_rviz;

  /**
   * @brief 临时点
   * 
   */
  geometry_msgs::PoseStamped this_pose;

  /**
   * @brief Construct a new path object
   * 
   */
  path(void);

  /**
   * @brief Destroy the path object
   * 
   */
  ~path();

  /**
   * @brief 添加路径点
   * 
   * @param this_pose 
   */
  void add_pose(geometry_msgs::PoseStamped this_pose);

  /**
   * @brief 删除所有路径点
   * 
   */
  void clear_path(void);

  /**
   * @brief 发布 path 消息到 rviz
   * 
   * @param this_path 
   */
  void publish_to_rviz(void); 
};

class obstacle
{
public:
  /**
   * @brief 线程 obstacle_rviz_handle
   * 
   */
  ros::NodeHandle obstacle_rviz_handle;

  /**
   * @brief 消息发布 obstacle_rviz_pub
   * 
   */
  ros::Publisher obstacle_rviz_pub;

  /**
   * @brief rviz 消息变量
   * 
   */
  visualization_msgs::Marker obstacle_rviz;

  /**
   * @brief 临时障碍物位置
   * 
   */
  geometry_msgs::Point this_obstacle;

  /**
   * @brief 添加障碍物
   * 
   */
  void add_obstacle(geometry_msgs::Point this_obstacle);

  /**
   * @brief 将障碍物发布到 rviz 中
   * 
   */
  void publish_to_rviz(void);

  /**
   * @brief 打印障碍物位置
   * 
   */
  void printf_obstacle_pose(void);

  /**
   * @brief Construct a new obstacle object
   * 
   */
  obstacle(void);

  /**
   * @brief Destroy the obstacle object
   * 
   */
  ~obstacle(void);
};

#endif